#ifndef UTILS
#define UTILS

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <ros/package.h>
#include<deque>
using namespace std;
using PointType = pcl::PointXYZI;

//img lidar calib
// string kb_inf_;

struct ObjectPc
{
    ros::Time time_stamp;
    string idx;
    pcl::PointCloud<PointType>::Ptr p_object_pc;
};

struct DataRet
{
    string idx;
    ros::Time time_stamp;
    PointType center;
    PointType lwh;
    cv::Mat bbox_corners;
};

template <typename T1, typename T2, typename T3>
void fixed_size_push_back(T1 &cur_pos, T2 data, T3 length)
{
    if (cur_pos.size() < length)
        cur_pos.push_back(data);
    else
    {
        cur_pos.pop_front();
        cur_pos.push_back(data);
    }
}

template <typename T>
void clear_vector(std::vector<T> &vt)
{
    std::vector<T> vtTemp;
    vtTemp.swap(vt);
}

template <typename Tq>
void clear_deque(std::deque<Tq> &q) {
	std::deque<Tq> empty;
	swap(empty, q);
}

string get_keyboard()
{
    char keyboard_input = cv::waitKey(30);
    
    string keyboard_inf;
    switch (keyboard_input){
    case 'z':{
        keyboard_inf = string("ammunition_box");
        break;
    }
    case 'x':{
        keyboard_inf = string("tank");
        break;
    }
    case 'c':{
        keyboard_inf = string("bomb");
        break;
    }
    case 'v':{
        keyboard_inf = string("person");
        break;
    }
    case ' ':{
        keyboard_inf = string("reset");
        break;
    }
    case '0':{
        keyboard_inf = string("0");
        break;
    }
    case '1':{
        keyboard_inf = string("1");
        break;
    }
    case '2':{
        keyboard_inf = string("2");
        break;
    }
    case '3':{
        keyboard_inf = string("3");
        break;
    }
    case '4':{
        keyboard_inf = string("4");
        break;
    }
    case '5':{
        keyboard_inf = string("5");
        break;
    }
    case '6':{
        keyboard_inf = string("6");
        break;
    }
    case '7':{
        keyboard_inf = string("7");
        break;
    }
    case '8':{
        keyboard_inf = string("8");
        break;
    }
    case '9':{
        keyboard_inf = string("9");
        break;
    }
    case 's':{
        keyboard_inf = string("save");
    }
    default:{
        break;
    }
    }
    return keyboard_inf;
}

void display_pc_for_measure(vector<DataRet> ret, ros::Publisher& pc_plane_pub_, ros::Publisher& bbox_plane_pub_)
{
    if (ret.size() == 0)
    {
        ROS_INFO_ONCE("no 3dbbox");
        return;
    }
    float life = 0.4;
    //3d bbox
    visualization_msgs::MarkerArray marker_array;
    geometry_msgs::Point p;
    PointType point_pc;
    pcl::PointCloud<PointType>::Ptr pc_plane(new pcl::PointCloud<PointType>);
    for (int i = 0; i < ret.size(); i++)
    {
        {
            point_pc.x = ret[i].bbox_corners.at<float>(0, 0);
            point_pc.y = ret[i].bbox_corners.at<float>(0, 1);
            point_pc.z = 0;
            point_pc.intensity = 5;
            pc_plane->push_back(point_pc);
            Eigen::Vector2f line_vec((ret[i].bbox_corners.at<float>(1, 0) - ret[i].bbox_corners.at<float>(0, 0)), (ret[i].bbox_corners.at<float>(1, 1) - ret[i].bbox_corners.at<float>(0, 1)));
            float line_vec_norm = line_vec.norm();
            line_vec.normalize();
            float length_exp = 0.01;
            while (length_exp < line_vec_norm)
            {
                point_pc.x = ret[i].bbox_corners.at<float>(0, 0) + line_vec(0) * length_exp;
                point_pc.y = ret[i].bbox_corners.at<float>(0, 1) + line_vec(1) * length_exp;
                pc_plane->push_back(point_pc);
                length_exp = length_exp + 0.01;
            }

            point_pc.x = ret[i].bbox_corners.at<float>(1, 0);
            point_pc.y = ret[i].bbox_corners.at<float>(1, 1);
            pc_plane->push_back(point_pc);
            line_vec = Eigen::Vector2f((ret[i].bbox_corners.at<float>(2, 0) - ret[i].bbox_corners.at<float>(1, 0)), (ret[i].bbox_corners.at<float>(2, 1) - ret[i].bbox_corners.at<float>(1, 1)));
            line_vec_norm = line_vec.norm();
            line_vec.normalize();
            length_exp = 0.01;
            while (length_exp < line_vec_norm)
            {
                point_pc.x = ret[i].bbox_corners.at<float>(1, 0) + line_vec(0) * length_exp;
                point_pc.y = ret[i].bbox_corners.at<float>(1, 1) + line_vec(1) * length_exp;
                pc_plane->push_back(point_pc);
                length_exp = length_exp + 0.01;
            }

            point_pc.x = ret[i].bbox_corners.at<float>(2, 0);
            point_pc.y = ret[i].bbox_corners.at<float>(2, 1);
            pc_plane->push_back(point_pc);
            line_vec = Eigen::Vector2f((ret[i].bbox_corners.at<float>(3, 0) - ret[i].bbox_corners.at<float>(2, 0)), (ret[i].bbox_corners.at<float>(3, 1) - ret[i].bbox_corners.at<float>(2, 1)));
            line_vec_norm = line_vec.norm();
            line_vec.normalize();
            length_exp = 0.01;
            while (length_exp < line_vec_norm)
            {
                point_pc.x = ret[i].bbox_corners.at<float>(2, 0) + line_vec(0) * length_exp;
                point_pc.y = ret[i].bbox_corners.at<float>(2, 1) + line_vec(1) * length_exp;
                pc_plane->push_back(point_pc);
                length_exp = length_exp + 0.01;
            }

            point_pc.x = ret[i].bbox_corners.at<float>(3, 0);
            point_pc.y = ret[i].bbox_corners.at<float>(3, 1);
            pc_plane->push_back(point_pc);
            line_vec = Eigen::Vector2f((ret[i].bbox_corners.at<float>(0, 0) - ret[i].bbox_corners.at<float>(3, 0)), (ret[i].bbox_corners.at<float>(0, 1) - ret[i].bbox_corners.at<float>(3, 1)));
            line_vec_norm = line_vec.norm();
            line_vec.normalize();
            length_exp = 0.01;
            while (length_exp < line_vec_norm)
            {
                point_pc.x = ret[i].bbox_corners.at<float>(3, 0) + line_vec(0) * length_exp;
                point_pc.y = ret[i].bbox_corners.at<float>(3, 1) + line_vec(1) * length_exp;
                pc_plane->push_back(point_pc);
                length_exp = length_exp + 0.01;
            }
        }

        visualization_msgs::Marker text_marker;
        text_marker.header.frame_id = "map";
        text_marker.header.stamp = ros::Time::now();

        text_marker.id = i + 2000;
        text_marker.action = text_marker.ADD;
        text_marker.lifetime = ros::Duration();
        // text_marker.lifetime = ros::Duration(life);
        text_marker.type = text_marker.TEXT_VIEW_FACING;

        p.x = ret[i].center.x;
        p.y = ret[i].center.y;
        p.z = -0.5;

        text_marker.pose.position.x = p.x;
        text_marker.pose.position.y = p.y;
        text_marker.pose.position.z = p.z;

        text_marker.text = ret[i].idx;

        text_marker.scale.x = 0.5;
        text_marker.scale.y = 0.5;
        text_marker.scale.z = 0.5;

        text_marker.color.r = 1.0;
        text_marker.color.g = 0.0;
        text_marker.color.b = 0.0;
        text_marker.color.a = 1.0;
        marker_array.markers.push_back(text_marker);
    }
    sensor_msgs::PointCloud2 pc_plane_display;
    pcl::toROSMsg(*pc_plane, pc_plane_display);
    pc_plane_display.header.frame_id = "map";
    pc_plane_pub_.publish(pc_plane_display);
    bbox_plane_pub_.publish(marker_array);
}

void display_pc_for_measure_one(vector<DataRet> ret, ros::Publisher& pc_plane_pub_)
{
    if (ret.size() == 0)
    {
        ROS_INFO_ONCE("no plane 3dbbox");
        return;
    }
    PointType point_pc;
    pcl::PointCloud<PointType>::Ptr pc_plane(new pcl::PointCloud<PointType>);

    {
        point_pc.x = ret.back().bbox_corners.at<float>(0, 0);
        point_pc.y = ret.back().bbox_corners.at<float>(0, 1);
        point_pc.z = 0;
        point_pc.intensity = 5;
        pc_plane->push_back(point_pc);
        Eigen::Vector2f line_vec((ret.back().bbox_corners.at<float>(1, 0) - ret.back().bbox_corners.at<float>(0, 0)), (ret.back().bbox_corners.at<float>(1, 1) - ret.back().bbox_corners.at<float>(0, 1)));
        float line_vec_norm = line_vec.norm();
        line_vec.normalize();
        float length_exp = 0.01;
        while (length_exp < line_vec_norm)
        {
            point_pc.x = ret.back().bbox_corners.at<float>(0, 0) + line_vec(0) * length_exp;
            point_pc.y = ret.back().bbox_corners.at<float>(0, 1) + line_vec(1) * length_exp;
            pc_plane->push_back(point_pc);
            length_exp = length_exp + 0.01;
        }

        point_pc.x = ret.back().bbox_corners.at<float>(1, 0);
        point_pc.y = ret.back().bbox_corners.at<float>(1, 1);
        pc_plane->push_back(point_pc);
        line_vec = Eigen::Vector2f((ret.back().bbox_corners.at<float>(2, 0) - ret.back().bbox_corners.at<float>(1, 0)), (ret.back().bbox_corners.at<float>(2, 1) - ret.back().bbox_corners.at<float>(1, 1)));
        line_vec_norm = line_vec.norm();
        line_vec.normalize();
        length_exp = 0.01;
        while (length_exp < line_vec_norm)
        {
            point_pc.x = ret.back().bbox_corners.at<float>(1, 0) + line_vec(0) * length_exp;
            point_pc.y = ret.back().bbox_corners.at<float>(1, 1) + line_vec(1) * length_exp;
            pc_plane->push_back(point_pc);
            length_exp = length_exp + 0.01;
        }

        point_pc.x = ret.back().bbox_corners.at<float>(2, 0);
        point_pc.y = ret.back().bbox_corners.at<float>(2, 1);
        pc_plane->push_back(point_pc);
        line_vec = Eigen::Vector2f((ret.back().bbox_corners.at<float>(3, 0) - ret.back().bbox_corners.at<float>(2, 0)), (ret.back().bbox_corners.at<float>(3, 1) - ret.back().bbox_corners.at<float>(2, 1)));
        line_vec_norm = line_vec.norm();
        line_vec.normalize();
        length_exp = 0.01;
        while (length_exp < line_vec_norm)
        {
            point_pc.x = ret.back().bbox_corners.at<float>(2, 0) + line_vec(0) * length_exp;
            point_pc.y = ret.back().bbox_corners.at<float>(2, 1) + line_vec(1) * length_exp;
            pc_plane->push_back(point_pc);
            length_exp = length_exp + 0.01;
        }

        point_pc.x = ret.back().bbox_corners.at<float>(3, 0);
        point_pc.y = ret.back().bbox_corners.at<float>(3, 1);
        pc_plane->push_back(point_pc);
        line_vec = Eigen::Vector2f((ret.back().bbox_corners.at<float>(0, 0) - ret.back().bbox_corners.at<float>(3, 0)), (ret.back().bbox_corners.at<float>(0, 1) - ret.back().bbox_corners.at<float>(3, 1)));
        line_vec_norm = line_vec.norm();
        line_vec.normalize();
        length_exp = 0.01;
        while (length_exp < line_vec_norm)
        {
            point_pc.x = ret.back().bbox_corners.at<float>(3, 0) + line_vec(0) * length_exp;
            point_pc.y = ret.back().bbox_corners.at<float>(3, 1) + line_vec(1) * length_exp;
            pc_plane->push_back(point_pc);
            length_exp = length_exp + 0.01;
        }
    }
    
    sensor_msgs::PointCloud2 pc_plane_display;
    pcl::toROSMsg(*pc_plane, pc_plane_display);
    pc_plane_display.header.frame_id = ret.back().idx;
    pc_plane_pub_.publish(pc_plane_display);
}

bool get_tf(string parent_frame, string child_frame, ros::Time time_stamp,tf::StampedTransform & get_tf)
{
    static tf::TransformListener listener;
    try{
        // listener.waitForTransform("/base_link", "/map", ros::Time(0), ros::Duration(0.3));
        // listener.lookupTransform("/base_link", "/map", ros::Time(0), T_lidar2map); // slam tf rate mast be good
        listener.waitForTransform(parent_frame, child_frame, time_stamp,ros::Duration(0.3));
        listener.lookupTransform(parent_frame, child_frame,  time_stamp, get_tf); // slam tf rate mast be good
        // cout<<"tf frame id  "<<T_lidar2map.frame_id_<<endl;
        // cout<<"get tf time: "<<float(T_lidar2map_.stamp_.toSec())<<endl;  //need simulation time. run lio-sam record tf topic
        // cout<<"tf "<<T_lidar2map.getOrigin().x()<<" "<<T_lidar2map.getOrigin().y()<<" "<<T_lidar2map.getOrigin().z()<<endl;
        // cout<<"tf  angle "<<T_lidar2map.getRotation().getAngle();
        return true;
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s", ex.what());
        return false;
    }
    //to do get manipulator tf
}

void display_bbox(vector<DataRet> ret, ros::Publisher&  bbox_pub_)
{
    if (ret.size() == 0)
    {
        ROS_INFO_ONCE("no 3dbbox");
        return;
    }
    float life = 0.4;
    //3d bbox
    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker marker;
    marker.header.frame_id = "map";
    marker.type = marker.LINE_LIST;
    marker.action = marker.ADD;
    marker.lifetime = ros::Duration();
    // marker.lifetime = ros::Duration(life);
    marker.header.stamp = ros::Time::now();

    //  marker scale (scale y and z not used due to being linelist)
    marker.scale.x = 0.08;
    //  marker color
    marker.color.a = 1.0;
    marker.color.r = 1.0;
    marker.color.g = 1.0;
    marker.color.b = 0.0;

    marker.pose.position.x = 0.0;
    marker.pose.position.y = 0.0;
    marker.pose.position.z = 0.0;

    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    // marker.points = []
    int corner_for_box_list[24] = {0, 1, 0, 3, 2, 3, 2, 1, 4, 5, 4, 7, 6, 7, 6, 5, 3, 7, 0, 4, 1, 5, 2, 6};
    geometry_msgs::Point p;
    for (int i = 0; i < ret.size(); i++)
    {
        cv::Mat one_detection_corners = ret[i].bbox_corners;
        for (int point_idx = 0; point_idx < 24; point_idx++)
        {
            p.x = one_detection_corners.at<float>(corner_for_box_list[point_idx], 0);
            p.y = one_detection_corners.at<float>(corner_for_box_list[point_idx], 1);
            p.z = one_detection_corners.at<float>(corner_for_box_list[point_idx], 2);
            marker.points.push_back(p);
        }
        visualization_msgs::Marker text_marker;
        text_marker.header.frame_id = "map";
        text_marker.header.stamp = ros::Time::now();

        text_marker.id = i + 1000;
        text_marker.action = marker.ADD;
        marker.lifetime = ros::Duration();
        // text_marker.lifetime = ros::Duration(life);
        text_marker.type = marker.TEXT_VIEW_FACING;

        p.x = ret[i].center.x;
        p.y = ret[i].center.y;
        p.z = ret[i].center.z;

        text_marker.pose.position.x = p.x;
        text_marker.pose.position.y = p.y;
        text_marker.pose.position.z = p.z + 0.8;

        text_marker.text = ret[i].idx;

        text_marker.scale.x = 0.5;
        text_marker.scale.y = 0.5;
        text_marker.scale.z = 0.5;

        text_marker.color.r = 1.0;
        text_marker.color.g = 0.0;
        text_marker.color.b = 0.0;
        text_marker.color.a = 1.0;
        marker_array.markers.push_back(text_marker);
    }
    marker_array.markers.push_back(marker);
    int id = 0;
    for (auto &m : marker_array.markers)
    {
        m.id = id;
        id += 1;
    }
    bbox_pub_.publish(marker_array);
}

void display_pc(pcl::PointCloud<PointType>::Ptr cloud, ros::Publisher& cloud_pub_){
    if (cloud->empty())
    {
        ROS_ERROR("display pc has no points");
        return;
    }

    sensor_msgs::PointCloud2 pc_display;
    pcl::toROSMsg(*cloud, pc_display);

    // pc_display.header.frame_id="odom";
    // geometry_msgs::TransformStamped trans;
    // trans.child_frame_id="bask_link";

    // trans.transform.rotation.x= T_lidar2map_.getRotation().x();   trans.transform.rotation.y= T_lidar2map_.getRotation().y();
    // trans.transform.rotation.z= T_lidar2map_.getRotation().z();   trans.transform.rotation.w= T_lidar2map_.getRotation().w();
    // trans.transform.translation.x= T_lidar2map_.getOrigin().x();
    // trans.transform.translation.y= T_lidar2map_.getOrigin().y();
    // trans.transform.translation.z= T_lidar2map_.getOrigin().z();

    // pc_display.header.frame_id = "map";
    // geometry_msgs::TransformStamped trans;
    // trans.child_frame_id = "object";

    // trans.transform.rotation.x = 0;
    // trans.transform.rotation.y = 0;
    // trans.transform.rotation.z = 0;
    // trans.transform.rotation.w = 1;
    // trans.transform.translation.x = 0;
    // trans.transform.translation.y = 0;
    // trans.transform.translation.z = 0;

    sensor_msgs::PointCloud2 cloud_pub;
    // tf2::doTransform(pc_display, cloud_pub, trans);
    // cloud_pub.header.frame_id = "map";
    // cloud_pub.header.stamp = ros::Time::now();
    pc_display.header.stamp = ros::Time::now();
    pc_display.header.frame_id = "map";

    cloud_pub_.publish(pc_display);
    // cloud_pub_.publish(cloud_pub);
}

pcl::PointCloud<PointType>::Ptr handle_local_map(tf::StampedTransform t_base2map, pcl::PointCloud<PointType>::Ptr p_pc, bool is_inverse){
    Eigen::Matrix4f T_map2base = Eigen::Matrix4f::Identity();
    Eigen::Quaternionf q_odom(t_base2map.getRotation().w(),t_base2map.getRotation().x(),
            t_base2map.getRotation().y(),t_base2map.getRotation().z());
    T_map2base.block<3, 3>(0, 0) = q_odom.toRotationMatrix();
    T_map2base.block<3, 1>(0, 3) = Eigen::Vector3f(t_base2map.getOrigin().x(), t_base2map.getOrigin().y(), t_base2map.getOrigin().z());
    if(is_inverse)
        T_map2base = T_map2base.inverse().eval();
    pcl::PointCloud<PointType>::Ptr p_ret(new pcl::PointCloud<PointType>);
    pcl::transformPointCloud(*p_pc, *p_ret, T_map2base);
    return p_ret;
}


pcl::PointCloud <PointType>::Ptr cut_pc(pcl::PointCloud<PointType>::Ptr pc, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax){
    pcl::PassThrough<PointType> pass;
    pcl::PointCloud<PointType>::Ptr pc_ret(new pcl::PointCloud<PointType>);
    pass.setInputCloud (pc);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("z");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (zmin, zmax);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_ret);//输出到结果指针
    pass.setInputCloud (pc_ret);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("y");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (ymin, ymax);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_ret);//输出到结果指针
    pass.setInputCloud (pc_ret);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("x");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (xmin, xmax);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_ret);//输出到结果指针
    return pc_ret;
}

void acumu_pc(const sensor_msgs::PointCloud2ConstPtr& laser_msg, pcl::PointCloud<PointType>::Ptr pc_acumu, 
    std::deque<pcl::PointCloud<PointType>> & cloudQueue, tf::StampedTransform t_base2map){
    pcl::PointCloud<PointType>::Ptr laser_cloud_in(new pcl::PointCloud<PointType>());
    pcl::fromROSMsg(*laser_msg, *laser_cloud_in);

    static tf::StampedTransform t_base2map_last_;
    tf::Vector3 dis_diff(t_base2map.getOrigin().x() - t_base2map_last_.getOrigin().x(), 
                                            t_base2map.getOrigin().y() - t_base2map_last_.getOrigin().y(), t_base2map.getOrigin().z() - t_base2map_last_.getOrigin().z());
    tf::Quaternion q_diff, q_last, q_cur;
    q_last  = tf::Quaternion(t_base2map_last_.getRotation().x(), t_base2map_last_.getRotation().y(),t_base2map_last_.getRotation().z(), t_base2map_last_.getRotation().w());
    q_cur  = tf::Quaternion(t_base2map.getRotation().x(), t_base2map.getRotation().y(),t_base2map.getRotation().z(), t_base2map.getRotation().w());
    q_diff = q_cur.inverse()*q_last;
    t_base2map_last_ = t_base2map;
    if(dis_diff.length()>0.05 || abs(q_diff.getAngle())>0.1){
        // cout<<"dis diff "<<dis_diff.length()<<"  q diff  "<<q_diff.getAngle()<<endl;
       laser_cloud_in = cut_pc(laser_cloud_in, 0, 10, -10, 10, -5, 5);
        // pcl::VoxelGrid<PointType> dw;
        // dw.setInputCloud(laser_cloud_in);
        // dw.setLeafSize(0.09f,0.09f,0.09f);
        // dw.filter(*laser_cloud_in);s
        laser_cloud_in = handle_local_map(t_base2map, laser_cloud_in, false);
        fixed_size_push_back(cloudQueue, *laser_cloud_in, 10);
    }

    // 2. downsample new cloud (save memory) 点云下采样
    // pcl::PointCloud<PointType>::Ptr laser_cloud_in_ds(new pcl::PointCloud<PointType>());
    // static pcl::VoxelGrid<PointType> downSizeFilter;
    // downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    // downSizeFilter.setInputCloud(laser_cloud_in);
    // downSizeFilter.filter(*laser_cloud_in_ds);
    // *laser_cloud_in = *laser_cloud_in_ds;

    // double timeScanCur = laser_msg->header.stamp.toSec();
    // cloudQueue.push_back(*laser_cloud_in);
    // timeQueue.push_back(timeScanCur);

    // 4. pop old cloud 保持队列里的点云的时长不超过一定的时间
    // while (!timeQueue.empty())
    // {
    //     if (timeScanCur - timeQueue.front() > 5.0)//超过1s的点云就提出队列
    //     {
    //         cloudQueue.pop_front();
    //         timeQueue.pop_front();
    //     } else {
    //         break;
    //     }
    // }

    // 5. fuse global cloud 将队列里的点云输入作为总体的待处理深度图
    for (int i = 0; i < (int)cloudQueue.size(); ++i)
        *pc_acumu += cloudQueue[i];

    // 6. downsample global cloud 深度图降采样
    // pcl::PointCloud<PointType>::Ptr depthCloudDS(new pcl::PointCloud<PointType>());
    // downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    // downSizeFilter.setInputCloud(depthCloud);
    // downSizeFilter.filter(*depthCloudDS);
    // *depthCloud = *depthCloudDS;
    // sensor_msgs::PointCloud2 depthCloudmsg;
}


#endif
